﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Doubao.DroneCoordinateCalculator
{
    public class DroneCoordinateCalculator
    {
        // 地球半径（米）
        private const double EarthRadius = 6378137.0;

        // 无人机姿态数据
        public double DroneRoll { get; set; }    // 横滚角（度）
        public double DroneYaw { get; set; }     // 偏航角（度）
        public double DronePitch { get; set; }   // 俯仰角（度）

        // 云台姿态数据
        public double GimbalRoll { get; set; }   // 横滚角（度）
        public double GimbalYaw { get; set; }    // 偏航角（度）
        public double GimbalPitch { get; set; }  // 俯仰角（度）

        // 无人机位置数据
        public double Longitude { get; set; }    // 经度（度）
        public double Latitude { get; set; }     // 纬度（度）
        public double Altitude { get; set; }     // 海拔高度（米）
        public double RelativeHeight { get; set; } // 相对高度（米）

        // 相机参数
        public int ImageWidth { get; set; }      // 图像宽度（像素）
        public int ImageHeight { get; set; }     // 图像高度（像素）
        public double FocalLength { get; set; }  // 焦距（毫米）
        public double PixelSize { get; set; }    // 像元尺寸（微米）

        // DSM地形文件路径
        public string DsmFilePath { get; set; }

        public DroneCoordinateCalculator()
        {
            // 默认初始化
            ImageWidth = 640;
            ImageHeight = 512;
            FocalLength = 24.0;
            PixelSize = 12.0;
        }

        // 计算图像中心的经纬度
        public (double longitude, double latitude) CalculateImageCenterCoordinates()
        {
            // 图像中心像素坐标
            double centerX = ImageWidth / 2.0;
            double centerY = ImageHeight / 2.0;

            return CalculateCoordinates(centerX, centerY);
        }

        // 计算标注目标物的经纬度（使用矩形的中心点）
        public (double longitude, double latitude) CalculateTargetCoordinates(int left, int top, int right, int bottom)
        {
            // 计算目标矩形的中心点
            double targetX = (left + right) / 2.0;
            double targetY = (top + bottom) / 2.0;

            return CalculateCoordinates(targetX, targetY);
        }

        // 计算任意像素点对应的经纬度
        private (double longitude, double latitude) CalculateCoordinates(double pixelX, double pixelY)
        {
            // 转换为相机坐标系（以图像中心为原点，向右为x正方向，向下为y正方向）
            double x = (pixelX - ImageWidth / 2.0) * (PixelSize / 1000.0);  // 毫米
            double y = (pixelY - ImageHeight / 2.0) * (PixelSize / 1000.0);  // 毫米

            // 相机坐标系下的归一化坐标（z=1平面）
            double nx = x / FocalLength;
            double ny = y / FocalLength;
            double nz = 1.0;

            // 构建旋转矩阵
            // 1. 相机坐标系到云台坐标系（相机通常固定在云台上）
            // 2. 云台坐标系到无人机坐标系
            // 3. 无人机坐标系到地理坐标系

            // 计算相机相对于地理坐标系的总旋转矩阵
            var rotationMatrix = CalculateRotationMatrix();

            // 应用旋转矩阵，将相机坐标系下的点转换到地理坐标系
            double vx = nx * rotationMatrix[0, 0] + ny * rotationMatrix[0, 1] + nz * rotationMatrix[0, 2];
            double vy = nx * rotationMatrix[1, 0] + ny * rotationMatrix[1, 1] + nz * rotationMatrix[1, 2];
            double vz = nx * rotationMatrix[2, 0] + ny * rotationMatrix[2, 1] + nz * rotationMatrix[2, 2];

            // 计算目标点的高度（考虑地形）
            double targetHeight = GetTerrainHeight(Longitude, Latitude);

            // 计算视线与地面的交点（考虑地形）
            double groundDistance = (Altitude - targetHeight) / vz;
            double dx = vx * groundDistance;
            double dy = vy * groundDistance;

            // 将地面上的相对偏移量转换为经纬度变化
            double dLat = (dy / EarthRadius) * (180.0 / Math.PI);
            double dLon = (dx / (EarthRadius * Math.Cos(Latitude * Math.PI / 180.0))) * (180.0 / Math.PI);

            // 计算最终经纬度
            double targetLat = Latitude + dLat;
            double targetLon = Longitude + dLon;

            return (targetLon, targetLat);
        }

        // 计算旋转矩阵
        private double[,] CalculateRotationMatrix()
        {
            // 将角度转换为弧度
            double droneRollRad = DroneRoll * Math.PI / 180.0;
            double droneYawRad = DroneYaw * Math.PI / 180.0;
            double dronePitchRad = DronePitch * Math.PI / 180.0;

            double gimbalRollRad = GimbalRoll * Math.PI / 180.0;
            double gimbalYawRad = GimbalYaw * Math.PI / 180.0;
            double gimbalPitchRad = GimbalPitch * Math.PI / 180.0;

            // 构建旋转矩阵（使用ZYX约定：偏航(Y)-俯仰(X)-横滚(Z)）
            // 1. 相机相对于云台的旋转（假设相机固定在云台上）
            double[,] cameraToGimbal = new double[3, 3];
            // 这里简化处理，假设相机与云台的相对姿态已包含在gimbalRoll, gimbalPitch, gimbalYaw中

            // 2. 云台相对于无人机的旋转
            double[,] gimbalToDrone = new double[3, 3];
            // 通常云台安装在无人机上，可能有固定偏移

            // 3. 无人机相对于地理坐标系的旋转
            double[,] droneToWorld = new double[3, 3];

            // 偏航旋转矩阵（绕Z轴）
            double[,] yawMatrix = new double[3, 3]
            {
                { Math.Cos(droneYawRad), -Math.Sin(droneYawRad), 0 },
                { Math.Sin(droneYawRad), Math.Cos(droneYawRad), 0 },
                { 0, 0, 1 }
            };

            // 俯仰旋转矩阵（绕X轴）
            double[,] pitchMatrix = new double[3, 3]
            {
                { 1, 0, 0 },
                { 0, Math.Cos(dronePitchRad), -Math.Sin(dronePitchRad) },
                { 0, Math.Sin(dronePitchRad), Math.Cos(dronePitchRad) }
            };

            // 横滚旋转矩阵（绕Y轴）
            double[,] rollMatrix = new double[3, 3]
            {
                { Math.Cos(droneRollRad), 0, Math.Sin(droneRollRad) },
                { 0, 1, 0 },
                { -Math.Sin(droneRollRad), 0, Math.Cos(droneRollRad) }
            };

            // 组合旋转矩阵：R = R_yaw * R_pitch * R_roll
            double[,] temp = MultiplyMatrices(yawMatrix, pitchMatrix);
            droneToWorld = MultiplyMatrices(temp, rollMatrix);

            // 计算总旋转矩阵（这里简化处理，实际应用中需要更复杂的组合）
            double[,] totalRotation = droneToWorld;

            return totalRotation;
        }

        // 矩阵乘法
        private double[,] MultiplyMatrices(double[,] a, double[,] b)
        {
            int rowsA = a.GetLength(0);
            int colsA = a.GetLength(1);
            int rowsB = b.GetLength(0);
            int colsB = b.GetLength(1);

            if (colsA != rowsB)
                throw new ArgumentException("矩阵维度不匹配");

            double[,] result = new double[rowsA, colsB];

            for (int i = 0; i < rowsA; i++)
            {
                for (int j = 0; j < colsB; j++)
                {
                    for (int k = 0; k < colsA; k++)
                    {
                        result[i, j] += a[i, k] * b[k, j];
                    }
                }
            }

            return result;
        }

        // 获取地形高度（使用DSM文件或默认值）
        private double GetTerrainHeight(double longitude, double latitude)
        {
            // 如果有DSM文件，从文件中获取地形高度
            if (!string.IsNullOrEmpty(DsmFilePath) && File.Exists(DsmFilePath))
            {
                try
                {
                    // 这里应该实现读取DSM文件的逻辑
                    // DSM文件通常是栅格格式，包含每个网格点的高程信息
                    // 这里简化处理，返回一个假设的地形高度
                    return ReadElevationFromDsm(longitude, latitude);
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"读取DSM文件失败: {ex.Message}");
                }
            }

            // 如果没有DSM文件或读取失败，使用相对高度计算
            return Altitude - RelativeHeight;
        }

        // 从DSM文件中读取指定经纬度的高程
        private double ReadElevationFromDsm(double longitude, double latitude)
        {
            // 这里应该实现DSM文件的解析逻辑
            // 简化示例，返回一个假设的地形高度
            return 3040.05; // 假设的地形高度，实际应用中应从DSM文件读取
        }


        public static void LMain()
        {
            try
            {
                // 创建计算器实例
                var calculator = new DroneCoordinateCalculator
                {
                    // 设置无人机姿态
                    DroneRoll = 15.0,
                    DroneYaw = 88.90,
                    DronePitch = -18.4,

                    // 设置云台姿态
                    GimbalRoll = 180.0,
                    GimbalYaw = -89.30,
                    GimbalPitch = -90.0,

                    // 设置无人机位置
                    Longitude = 100.20005,
                    Latitude = 36.003380555555559,
                    Altitude = 3109.71,
                    RelativeHeight = 69.662,

                    // 设置相机参数
                    ImageWidth = 640,
                    ImageHeight = 512,
                    FocalLength = 24.0,
                    PixelSize = 12.0,

                    // 设置DSM文件路径（如果有）
                    DsmFilePath = @"C:\path\to\dsm_file.tif"
                };

                // 计算图像中心的经纬度
                var centerCoords = calculator.CalculateImageCenterCoordinates();
                Console.WriteLine($"图像中心经纬度: 经度={centerCoords.longitude:F6}, 纬度={centerCoords.latitude:F6}");

                // 计算标注目标物的经纬度
                var targetCoords = calculator.CalculateTargetCoordinates(169, 253, 192, 296);
                Console.WriteLine($"目标物经纬度: 经度={targetCoords.longitude:F6}, 纬度={targetCoords.latitude:F6}");

                Console.WriteLine("按任意键退出...");
                Console.ReadKey();
            }
            catch (Exception ex)
            {
                Console.WriteLine($"计算过程中发生错误: {ex.Message}");
                Console.WriteLine(ex.StackTrace);
                Console.WriteLine("按任意键退出...");
                Console.ReadKey();
            }
        }
    }

    
}